from rclpy.node import Node
# import all message types
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import PoseStamped
from object_recognition_msgs.msg import RecognizedObjectArray
from std_msgs.msg import Empty, String
from rcl_interfaces.msg import Log, ParameterEvent
from visualization_msgs.msg import InteractiveMarkerFeedback, InteractiveMarkerUpdate, MarkerArray
from sensor_msgs.msg import CameraInfo, Image, JointState, PointCloud2
from moveit_msgs.msg import AttachedCollisionObject, CollisionObject, DisplayTrajectory, MotionPlanRequest, PlanningScene, PlanningSceneWorld, RobotState


class Publisher(Node):

    def __init__(self, name, msg_typ, topic, timer_msg=None, timer_period=0):
        super().__init__(name)
        self.timer_msg = timer_msg
        self.command_publisher = self.create_publisher(msg_typ, topic, 10)
        if timer_period > 0:
            self.timer = self.create_timer(timer_period, self.timer_publish)
        self.get_logger().info("publishing node %s has been created." % name)

    def timer_publish(self):
        msg = self.timer_msg
        self.command_publisher.publish(msg)
        self.get_logger().info(f'automatic message sent: {msg}')

    def normal_publish(self, msg):
        self.command_publisher.publish(msg)
        self.get_logger().info(f'manual message sent: {msg}')


class Subscriber(Node):
    def __init__(self, name, msg_typ, topic):
        super().__init__(name)
        self.topic = topic
        self.message = None
        self.command_subscribe = self.create_subscription(
            msg_typ, topic, self.callback, 10)
        self.get_logger().info("subscribing node %s has been created." % name)

    def callback(self, msg):
        self.get_logger().info(
            f'under {self.topic} topic, message received: {msg}')
        self.message = msg
